/*
 * T_Junction_1.c
 *
 * Created: 6/11/2013 9:11:25 PM
 *  Author: Crossfire
 */

#include "T-Junction_1.h"
#include "Globals.h"

void get_Sensors1()
{
	if (PINA & (1<<0) )
	{
		senArray1[0] = 1;
	}
	else
	{
		senArray1[0]= 0;
	}

	if (PINA & (1<<1))
	{
		senArray1[1] = 1;
	}
	else
	{
		senArray1[1]=0;
	}

	if (PINA & (1<<2))
	{
		senArray1[2] = 1;
	}
	else
	{
		senArray1[2]=0;
	}

}

void set_State1()
{
	int tellerEen, tellerTwee, tellerDrie, tellerVier, tellerVijf, tellerZes, tellerZeven, tellerAcht, tellerNegen, tellerTien, tellerElf;
	int staArray1[7][4] =
	{
		{1,0,0,2},
		{0,1,0,4},
		{0,0,1,6},
		{1,1,1,4},
		{1,1,0,2},
		{0,1,1,6},
		{1,0,1,2}
	};
	if(presentState1==1)
	{
		tellerVier=1;

		for(tellerEen=0; tellerEen<7; tellerEen++)
		{
			tellerDrie=0;
			for(tellerTwee=0; tellerTwee<3; tellerTwee++)
			{
				if(senArray1[tellerTwee]==staArray1[tellerEen][tellerTwee])
				{
					tellerDrie++;
					if(tellerDrie==3)
					{
						tellerVier=staArray1[tellerEen][3];
						if(tellerVier!=0 && tellerEen==0)
						{
							tellerVijf=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==1)
						{
							tellerZes=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==2)
						{
							tellerZeven=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==3)
						{
							tellerAcht=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==4)
						{
							tellerNegen=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==5)
						{
							tellerTien=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==6)
						{
							tellerElf=tellerVier;
						}


					}
				}
			}
		}
	}

	else if(presentState1==2)
	{
		tellerVier=2;
		int staArray2[7][4] =
		{
		{1,0,1,3},
		{0,1,1,3},
		{0,0,0,3},
		{0,0,1,3},
		{0,1,0,3},
		{1,1,0,3},
		{1,1,1,3},
		};
		for(tellerEen=0; tellerEen<7; tellerEen++)
		{
			tellerDrie=0;
			for(tellerTwee=0; tellerTwee<3; tellerTwee++)
			{
				if(senArray1[tellerTwee]==staArray2[tellerEen][tellerTwee])
				{
					tellerDrie++;
					if(tellerDrie==3)
					{
						tellerVier=staArray2[tellerEen][3];
						if(tellerVier!=0 && tellerEen==0)
						{
							tellerVijf=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==1)
						{
							tellerZes=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==2)
						{
							tellerZeven=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==3)
						{
							tellerAcht=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==4)
						{
							tellerNegen=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==5)
						{
							tellerTien=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==5)
						{
							tellerElf=tellerVier;
						}
					}
				}
			}
		}
	}

	else if(presentState1==3)
	{
		int staArray3[8][4] =
		{
		{0,0,0,1},
		{1,0,0,1},
		{1,1,0,1},
		{1,1,1,1},
		{0,1,1,1},
		{0,0,1,1},
		{0,1,0,1},
		{1,0,1,1},
		};
		for(tellerEen=0; tellerEen<8; tellerEen++)
		{
			tellerDrie=0;
			for(tellerTwee=0; tellerTwee<3; tellerTwee++)
			{
				if(senArray1[tellerTwee]==staArray3[tellerEen][tellerTwee])
				{
					tellerDrie++;
					if(tellerDrie==3)
					{
						tellerVier=staArray3[tellerEen][3];
					}
				}
			}
		}

		staArray1[3][3] = 4;
		staArray1[4][3] = 4;
		staArray1[6][3] = 6;

	}

	else if(presentState1==4)
	{
		tellerVier=4;
		int staArray4[7][4] =
		{
		{0,1,1,5},
		{1,0,1,5},
		{0,0,0,5},
		{0,0,1,5},
		{1,0,0,5},
		{1,1,0,5},
		{1,1,1,5},
		};
		for(tellerEen=0; tellerEen<7; tellerEen++)
		{
			tellerDrie=0;
			for(tellerTwee=0; tellerTwee<3; tellerTwee++)
			{
				if(senArray1[tellerTwee]==staArray4[tellerEen][tellerTwee])
				{
					tellerDrie++;
					if(tellerDrie==3)
					{
						tellerVier=staArray4[tellerEen][3];
						if(tellerVier!=0 && tellerEen==0)
						{
							tellerVijf=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==1)
						{
							tellerZes=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==2)
						{
							tellerZeven=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==3)
						{
							tellerAcht=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==4)
						{
							tellerNegen=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==5)
						{
							tellerTien=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==5)
						{
							tellerElf=tellerVier;
						}
					}
				}
			}
		}
	}

	else if(presentState1==5)
	{


		int staArray5[8][4] =
		{
		{0,0,0,1},
		{1,0,0,1},
		{1,1,0,1},
		{1,1,1,1},
		{0,1,1,1},
		{0,0,1,1},
		{0,1,0,1},
		{1,0,1,1},
		};
		for(tellerEen=0; tellerEen<8; tellerEen++)
		{
			tellerDrie=0;
			for(tellerTwee=0; tellerTwee<3; tellerTwee++)
			{
				if(senArray1[tellerTwee]==staArray5[tellerEen][tellerTwee])
				{
					tellerDrie++;
					if(tellerDrie==3)
					{
						tellerVier=staArray5[tellerEen][3];
					}
				}
			}
		}

		staArray1[3][3] = 6;
		staArray1[4][3] = 2;
		staArray1[5][3] = 6;

	}

	else if(presentState1==6)
	{
		tellerVier=6;
		int staArray6[7][4] =
		{
		{0,1,1,7},
		{1,1,0,7},
		{0,0,0,7},
		{0,1,0,7},
		{1,0,0,7},
		{1,0,1,7},
		{1,1,1,7}
		};
		for(tellerEen=0; tellerEen<7; tellerEen++)
		{
			tellerDrie=0;
			for(tellerTwee=0; tellerTwee<3; tellerTwee++)
			{
				if(senArray1[tellerTwee]==staArray6[tellerEen][tellerTwee])
				{
					tellerDrie++;
					if(tellerDrie==3)
					{
						tellerVier=staArray6[tellerEen][3];
						if(tellerVier!=0 && tellerEen==0)
						{
							tellerVijf=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==1)
						{
							tellerZes=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==2)
						{
							tellerZeven=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==3)
						{
							tellerAcht=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==4)
						{
							tellerNegen=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==5)
						{
							tellerTien=tellerVier;
						}
						else if(tellerVier!=0 && tellerEen==5)
						{
							tellerElf=tellerVier;
						}
					}
				}
			}
		}
	}

	else if(presentState1==7)
	{


		int staArray7[8][4] =
		{
		{0,0,0,1},
		{1,0,0,1},
		{1,1,0,1},
		{1,1,1,1},
		{0,1,1,1},
		{0,0,1,1},
		{0,1,0,1},
		{1,0,1,1},
		};
		for(tellerEen=0; tellerEen<8; tellerEen++)
		{
			tellerDrie=0;
			for(tellerTwee=0; tellerTwee<3; tellerTwee++)
			{
				if(senArray1[tellerTwee]==staArray7[tellerEen][tellerTwee])
				{
					tellerDrie++;
					if(tellerDrie==3)
					{
						tellerVier=staArray7[tellerEen][3];
					}
				}
			}
		}

		staArray1[3][3] = 2;
		staArray1[5][3] = 4;
		staArray1[6][3] = 2;

	}


	if(tellerVier!=0 && tellerVijf==0 && tellerZes==0 && tellerZeven==0 && tellerAcht==0)
	{
		presentState1=tellerVier;
	}
	if(tellerVijf!=0)
	{
		presentState1=tellerVijf;
	}
	else if(tellerZes!=0)
	{
		presentState1=tellerZes;
	}
	else if(tellerZeven!=0)
	{
		presentState1=tellerZeven;
	}
	else if(tellerAcht!=0)
	{
		presentState1=tellerAcht;
	}
	else if(tellerNegen!=0)
	{
		presentState1=tellerNegen;
	}
	else if(tellerTien!=0)
	{
		presentState1=tellerTien;
	}
	else if(tellerElf!=0)
	{
		presentState1=tellerElf;
	}
}

void set_Trafficlights1()
{
	int stoplichten1[3];
    if(_mode == 0)
    {
        for(int i = 0; i < 3; i++)
        {
            stoplichten1[i] = _Default1[presentState1][i];
        }
    }
    else
    {
        for(int i = 0; i < 3; i++)
        {
            stoplichten1[i] = TJunction[presentState1][i];
        }
    }

	//------------------------------------------------------------------------------
	if (stoplichten1[0] == 1)
	{
		orByteB |= 0b00000010;
	}
	if (stoplichten1[0] == 2)
	{
		orByteB |= 0b00000100;
	}
	if (stoplichten1[0] == 3)
	{
		orByteB |= 0b00001000;
	}

	//------------------------------------------------------------------------------
	if (stoplichten1[1] == 1)
	{
		orByteA |=  0b10000000;
	}
	if (stoplichten1[1] == 2)
	{
		orByteA |=  0b01000000;
	}
	if (stoplichten1[1] == 3)
	{
		orByteB |=  0b00000001;
	}

	//-------------------------------------------------------------------------------
	if (stoplichten1[2] == 1)
	{
		orByteC |=  0b00010000;
	}
	if (stoplichten1[2] == 2)
	{
		orByteC |=  0b00001000;
	}
	if (stoplichten1[2] == 3)
	{
		orByteC |=  0b00000100;
	}
}

void set_Delay1()
{
	if(orByteB == 0b00001000 || orByteB == 0b00000011 || orByteC == 0b00000100)
	{
		vertraging1 = 4;
	}
	else if(orByteB == 0b00000100 || orByteA == 0b10000000 || orByteC == 0b00001000)
	{
		vertraging1 = 2;
	}
	else
	{
		vertraging1 = 1;
	}
}

